﻿using System.Linq;
using Microsoft.Kinect;


namespace Schuifpuzzel.MyKinect
{
    public delegate void DistanceFromKinectHandler(int distance);
    public delegate void SkeletonAndImageFramesReady(Skeleton skeleton, ColorImageFrame colorImageFrame, DepthImageFrame depthFrame);

    public delegate void KinectStatusChanged(KinectStatus newKinectStatus);

    /// <summary>
    /// This class manages the kinect and allows other classes to bind to certain events.
    /// </summary>
    class KinectForPuzzle
    {
        private static KinectForPuzzle _puzzleInstance;
        private Skeleton[] _frameSkeletons;
        private KinectSensor _kinect;
        private GestureDetector _gestureDetector;
        private int _lastDetectedSkeleton;
        private bool _skeletonLocked;

        public event GestureDetectedHandler GestureDetected;
        public event ArmStretchedDetected ArmStretchedDetected;
        public event DistanceFromKinectHandler DistanceFromKinectDetected;
        public event SkeletonAndImageFramesReady SkeletonAndImageFramesReady;
        public event ArmPositionDetected ArmPositionDetected;
        public event KinectStatusChanged KinectStatusChanged;

        /// <summary>
        /// The hand that is being tracked
        /// </summary>
        public Hand TrackedHand
        {
            get { return _gestureDetector.TrackedHand; }
            set { _gestureDetector.TrackedHand = value; }
        }

        /// <summary>
        /// Indicates whether we are sticking to our tracked skeleton
        /// </summary>
        public bool SkeletonLocked
        {
            get { return _skeletonLocked; }
            set { _skeletonLocked = value;}
        }
        /// <summary>
        /// The representation of the kinect hardware
        /// </summary>
        public KinectSensor Kinect
        {
            get { return _kinect; }
            set
            {
                if (_kinect != value)
                {
                    if (_kinect != null)
                    {
                        UninitializeKinectSensor(_kinect);
                        _kinect = null;
                    }

                    if (value != null && value.Status == KinectStatus.Connected)
                    {
                        _kinect = value;
                        InitializeKinectSensor(_kinect);
                    }
                }
            }
        }

        /// <summary>
        /// The method for accessing the singleton Kinect
        /// </summary>
        /// <returns></returns>
        public static KinectForPuzzle GetInstance()
        {
            return _puzzleInstance ?? (_puzzleInstance = new KinectForPuzzle());
        }

        private KinectForPuzzle()
        {
            _gestureDetector = new GestureDetector();
            DiscoverKinectSensor();
            _gestureDetector.GestureDetected += GestureDetectorGestureDetected;
            _gestureDetector.ArmStretched += GestureDetectorArmStretched;
            _gestureDetector.ArmPositionDetected += GestureDetector_ArmPositionDetected;
        }

        /// <summary>
        /// Raises the ArmPositionDetected event
        /// </summary>
        /// <param name="handIsInFrontOfBody">Indicates whether the hand is in front of the body</param>
        /// <param name="xCoordinate">The xCoordinate of the tracked hand</param>
        /// <param name="yCoordinate">The yCoordinate of the tracked hand</param>
        /// <param name="detectingGesture">Indicates whether we are detecting a gesture or waiting for the hand to move back into start position</param>
        void GestureDetector_ArmPositionDetected(bool handIsInFrontOfBody, double xCoordinate, double yCoordinate, bool detectingGesture)
        {
            if(ArmPositionDetected != null)
            {

                ArmPositionDetected(handIsInFrontOfBody, xCoordinate, yCoordinate, detectingGesture);
            }
        }

        
        private void InitializeKinectSensor(KinectSensor sensor)
        {
            if (sensor == null) return;
            //Enable the streams that you wish to use
            sensor.DepthStream.Enable();
            sensor.ColorStream.Enable();

            //The parameters for changing the way the skeleton movement is reflected
            var parameters = new TransformSmoothParameters
            {
                Smoothing = 0.0f,
                Correction = 0.0f,
                Prediction = 0.0f,
                JitterRadius = 0.25f,
                MaxDeviationRadius = 0.125f
            };
            sensor.SkeletonStream.Enable(parameters);

            //Initialize array for the skeletons once, for performance
            _frameSkeletons = new Skeleton[Kinect.SkeletonStream.FrameSkeletonArrayLength];

            //Bind the AllFramesReady event
            sensor.AllFramesReady += KinectAllFramesReady;
            sensor.Start();
        }

        private void UninitializeKinectSensor(KinectSensor sensor)
        {
            if (sensor != null)
            {
                sensor.Stop();
                sensor.AllFramesReady -= KinectAllFramesReady;
                sensor.DepthStream.Disable();
                sensor.ColorStream.Disable();
                sensor.SkeletonStream.Disable();
            }
        }

        /// <summary>
        /// Gets the first connected Kinect
        /// </summary>
        private void DiscoverKinectSensor()
        {
            KinectSensor.KinectSensors.StatusChanged += KinectSensorStatusChanged;
            Kinect = KinectSensor.KinectSensors.FirstOrDefault(x => x.Status == KinectStatus.Connected);
        }

        /// <summary>
        /// Method called when the KinectSensor changes status
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void KinectSensorStatusChanged(object sender, StatusChangedEventArgs e)
        {
           
            switch (e.Status)
            {
                case KinectStatus.Connected:
                    if (Kinect == null)
                    {
                        Kinect = e.Sensor;
                    }
                    break;

                case KinectStatus.Disconnected:
                    if (Kinect == e.Sensor)
                    {
                        Kinect = null;
                        Kinect = KinectSensor.KinectSensors.FirstOrDefault(x => x.Status == KinectStatus.Connected);

                    }
                    break;

                //Add code to handle other statuses if needed

            }

            //Raise event to inform Kinect status has changed
            if (KinectStatusChanged != null)
            {
                KinectStatusChanged(e.Status);
            }
        }


        private void KinectAllFramesReady(object sender, AllFramesReadyEventArgs e)
        {
            if (Kinect == null) return; //Sometimes the event is still raised after the kinect has been uninitialized
            SkeletonFrame skeletonframe = e.OpenSkeletonFrame();
            DepthImageFrame depthFrame = e.OpenDepthImageFrame();
            ColorImageFrame imageFrame =  e.OpenColorImageFrame();

           
            if (skeletonframe != null && depthFrame != null) //Make sure the frames aren't null before using
            {
                skeletonframe.CopySkeletonDataTo(_frameSkeletons); //Retrieve all the skeletons to our array
                Skeleton skeleton;

                //If skeleton is locked, try to retrieve the last detected skeleton
                //Otherwise get the closestskeleton
                if(_skeletonLocked)
                {
                    skeleton = GetSkeletonById(_lastDetectedSkeleton) ?? GetClosestSkeleton(depthFrame);
                }
                else
                {
                    skeleton = GetClosestSkeleton(depthFrame);                  
                }

                
                if (SkeletonAndImageFramesReady != null && imageFrame != null)
                {
                    //Raise event that passes the skeleton, imageFrame and depthFrame so they can be used
                    SkeletonAndImageFramesReady(skeleton, imageFrame, depthFrame);
                    imageFrame.Dispose();
                }

                DetectDistance(skeleton, depthFrame);

                if (skeleton != null)
                { 
                    //Pass the skeleton and depethFrame to the GestureDetector so gestures can be detected
                    _gestureDetector.DetectGesture(skeleton, depthFrame);
                }

                skeletonframe.Dispose();
                depthFrame.Dispose();
                
            }

        }

        private Skeleton GetSkeletonById(int id)
        {
            Skeleton skeleton = (from s in _frameSkeletons
                                           where s.TrackingState == SkeletonTrackingState.Tracked && s.TrackingId==id
                                           select s).FirstOrDefault();

            return skeleton;
        }


        //Gets the nearest skeleton
        private Skeleton GetClosestSkeleton(DepthImageFrame depthFrame)
        {
            Skeleton closestSkeleton = null;
            Skeleton[] trackedSkeletons = (from s in _frameSkeletons
                                           where s.TrackingState == SkeletonTrackingState.Tracked
                                           select s).ToArray<Skeleton>();

            

            foreach (Skeleton skeleton in trackedSkeletons)
            {
                if (closestSkeleton == null)
                {
                    closestSkeleton = skeleton;
                }
                else
                {
                    Joint spineJoint = skeleton.Joints[JointType.Spine];
                        if (spineJoint.TrackingState == JointTrackingState.Tracked)
                        {
                            DepthImagePoint skeletonDepthPoint = depthFrame.MapFromSkeletonPoint(spineJoint.Position);
                            DepthImagePoint closestSkeletonDepthPoint = depthFrame.MapFromSkeletonPoint(closestSkeleton.Position);
                            if (skeletonDepthPoint.Depth < closestSkeletonDepthPoint.Depth)
                            {
                                closestSkeleton = skeleton;
                            }
                        }
                }
                
            }
            if (closestSkeleton != null)
            {
                _lastDetectedSkeleton = closestSkeleton.TrackingId;
            }

            return closestSkeleton;
        }

        /// <summary>
        /// Calculates the distance between the skeleton and the Kinect
        /// and calls the DistanceFromKinectDetected event with the calculated distance
        /// </summary>
        /// <param name="skeleton">The tracked skeleton</param>
        /// <param name="depthFrame">The depthFrame that contains the depth values</param>
        private void DetectDistance(Skeleton skeleton, DepthImageFrame depthFrame)
        {
            int distance = -1;
            if(skeleton != null)
            {
                Joint spineJoint = skeleton.Joints[JointType.Spine];
            
                    if (spineJoint.TrackingState == JointTrackingState.Tracked)
                    {
                        DepthImagePoint spineDepthPoint = depthFrame.MapFromSkeletonPoint(spineJoint.Position);
                        distance = spineDepthPoint.Depth;
                    }
            }
            if (DistanceFromKinectDetected != null)
            {
                DistanceFromKinectDetected(distance);
            }
        }

        void GestureDetectorGestureDetected(Model.MoveDirection detectedMovement)
        {
            if (GestureDetected != null)
                GestureDetected(detectedMovement);
        }

        void GestureDetectorArmStretched(Hand hand)
        {
            if (ArmStretchedDetected != null)
                ArmStretchedDetected(hand);
        }

        
    }
}
